#include "cv.h"
#include <highgui.h>
#include "opencv2/imgproc/imgproc.hpp"
#include "cv_bridge/cv_bridge.h"
#include <image_transport/image_transport.h>

#include "gr8/TagRecognition.h"

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int8MultiArray.h>

#include <string.h>
#include <signal.h>

using namespace cv;
using namespace gr8;

ros::Publisher recognitionCam0_pub;
ros::Publisher recognitionCam1_pub;

/*
This node is doing :
	-> Receive images with only the tag from tag detector
	-> Color analysis
	-> Resize it and make it binary
	-> Do image recognition using template matching
	-> Publish information in recognition_Cam0 and recognition_Cam1 topics
*/


#define BLACK                    0  
#define BLUE            	 1  
#define GREEN                    2  
#define MAGENTA		   	 3 

#define APPLE 			 10 
#define MUG			 11
#define TEDDY			 12  
#define GLASSES			 13
#define DRYER 			 14 
#define LAPTOP			 15
#define GLASS			 16  
#define HAMMER			 17
#define BANANA 			 18 
#define BOOK			 19
#define SCISSORS		 20  
#define CAMERA			 21

int maxValue = 255;
int color = -1;
int object = -1;

float orientation;

/* Function to look at black in the image */
int blackAnalysis (Mat Blue, Mat Green, Mat Red)
{
	double thresholdBlack = 30;
	threshold(Red, Red, thresholdBlack, maxValue, THRESH_BINARY_INV);
	threshold(Green, Green, thresholdBlack, maxValue, THRESH_BINARY_INV);
	threshold(Blue, Blue, thresholdBlack, maxValue, THRESH_BINARY_INV);

	Mat out;
	// Combine the masks
	bitwise_and(Red, Green, out);
	bitwise_and(out, Blue, out);

	int n = countNonZero(out);
	if (n>250) 
	{	
		return 1;
	} else 
	{
		return 0;
	}
}

/* Function to look at green in the image */
int greenAnalysis (Mat Blue, Mat Green, Mat Red, Mat Hue, Mat Sat)
{
	double thresholdGreen = 50;

	threshold(Red, Red, thresholdGreen, maxValue, THRESH_BINARY);
	threshold(Green, Green, thresholdGreen, maxValue, THRESH_BINARY_INV);
	threshold(Blue, Blue, thresholdGreen, maxValue, THRESH_BINARY);

	threshold(Hue, Hue, 128, maxValue, THRESH_BINARY | THRESH_OTSU);
	threshold(Sat, Sat, 128, maxValue, THRESH_BINARY_INV | THRESH_OTSU);

	Mat out;
	// Combine the masks
	bitwise_and(Red, Green, out);
	bitwise_and(out, Blue, out);
	bitwise_and(out, Hue, out);
	bitwise_and(out, Sat, out);

	int n = countNonZero(out);
	if (n>200) 
	{
		return 1;
	} else 
	{
		return 0;
	}
}

/* Function to look at blue in the image */
int blueAnalysis (Mat Blue, Mat Green, Mat Red, Mat Hue, Mat Sat)
{
	double thresholdBlue = 50;

	threshold(Red, Red, thresholdBlue, maxValue, THRESH_BINARY);
	threshold(Green, Green, thresholdBlue, maxValue, THRESH_BINARY_INV);
	threshold(Blue, Blue, thresholdBlue, maxValue, THRESH_BINARY_INV);

	threshold(Sat, Sat, 128, maxValue, THRESH_BINARY | THRESH_OTSU);

	Mat out;
	// Combine the masks
	bitwise_and(Red, Green, out);
	bitwise_and(out, Blue, out);
	//bitwise_and(out, Sat, out);

	int n = countNonZero(out);
	if (n>200) 
	{
		return 1;
	} else 
	{
		return 0;
	}
}

/* Function to look at magenta in the image */
int magentaAnalysis (Mat Blue, Mat Green, Mat Red, Mat Hue, Mat Sat)
{
	double thresholdMagenta = 50;

	threshold(Red, Red, thresholdMagenta, maxValue, THRESH_BINARY_INV);
	threshold(Green, Green, thresholdMagenta, maxValue, THRESH_BINARY);
	threshold(Blue, Blue, thresholdMagenta, maxValue, THRESH_BINARY);

	threshold(Hue, Hue, 128, maxValue, THRESH_BINARY | THRESH_OTSU);

	Mat out;
	// Combine the masks
	bitwise_and(Red, Green, out);
	bitwise_and(out, Blue, out);
	bitwise_and(out, Hue, out);

	int n = countNonZero(out);
	if (n>300) 
	{
		return 1;
	} else 
	{
		return 0;
	}
}


/* Function to find the color of the tag */
void colorAnalysis(Mat imgMat) 
{
	//Split the color channels
	vector<Mat> channels(3);
	split(imgMat, channels);
	Mat Blue = channels[0];
	Mat Green = channels[1];
	Mat Red = channels[2];

	// Convert into HSV and split
	Mat imgHSV;
	cvtColor(imgMat, imgHSV, CV_BGR2HSV, 0);
	vector<Mat> channelsHSV(3);
	split(imgHSV, channelsHSV);
	Mat Hue = channelsHSV[1];
	Mat Sat = channelsHSV[0];
	Mat Val = channelsHSV[2];


	// Is it black ?
	if (blackAnalysis(Blue, Green, Red))
	{
		color = 0;
	}
	

	// Is it green ?
	if (greenAnalysis(Blue, Green, Red, Hue, Sat))
	{
		color = 2;
	}

	// Is it blue ?
	if (blueAnalysis(Blue, Green, Red, Hue, Sat))
	{
		color = 1;
	}

	// Is it magenta ?
	if (magentaAnalysis(Blue, Green, Red, Hue, Sat) && color==-1)
	{
		color = 3;
	}

	switch(color)
	{
		case 0: printf("Color is black\n"); break;
		case 1: printf("Color is blue\n"); break;
		case 2: printf("Color is green\n"); break;
		case 3: printf("Color is magenta\n"); break;
		case -1: printf("Color is unknown\n"); color=50; break;
	}
}

/* Function to do object recognition using template matching */
void templateMatching(Mat img) 
{	
	int match_method = CV_TM_SQDIFF_NORMED;

	char* templateList[] = {"TemplateImagesNew/apple_1.jpg", "TemplateImagesNew/apple_2.jpg",
				   "TemplateImagesNew/mug_1.jpg", "TemplateImagesNew/mug_2.jpg",
				   "TemplateImagesNew/teddy_1.jpg", "TemplateImagesNew/teddy_2.jpg",
				   "TemplateImagesNew/glasses_1.jpg", "TemplateImagesNew/glasses_2.jpg",
				   "TemplateImagesNew/dryer_1.jpg", "TemplateImagesNew/dryer_2.jpg",
				   "TemplateImagesNew/laptop_1.jpg", "TemplateImagesNew/laptop_2.jpg",
				   "TemplateImagesNew/glass_1.jpg", "TemplateImagesNew/glass_2.jpg",
				   "TemplateImagesNew/hammer_1.jpg", "TemplateImagesNew/hammer_2.jpg",
				   "TemplateImagesNew/banana_1.jpg", "TemplateImagesNew/banana_2.jpg",
				   "TemplateImagesNew/book_1.jpg", "TemplateImagesNew/book_2.jpg",
				   "TemplateImagesNew/scissors_1.jpg", "TemplateImagesNew/scissors_2.jpg",
				   "TemplateImagesNew/camera_1.jpg", "TemplateImagesNew/camera_2.jpg"
				};

	Mat templ1, templ2, templ3;
	Mat result;
	float matchResult[12] = {1,1,1,1,1,1,1,1,1,1,1,1};

	IplImage *templ1Im;
	IplImage *templ2Im;
	IplImage *templ3Im;

	for (int i =0; i<12; i++)
	{
		templ1 = imread(templateList[2*i],CV_LOAD_IMAGE_GRAYSCALE);
		templ2 = imread(templateList[2*i+1],CV_LOAD_IMAGE_GRAYSCALE);

		/// Do the Matching
  		float tempResult;

		matchTemplate( img, templ1, result, match_method );
		tempResult=result.at<float>(0,0);

		matchTemplate( img, templ2, result, match_method );
		if (result.at<float>(0,0)<tempResult) 
		{
			tempResult=result.at<float>(0,0);
		}

		matchResult[i] = tempResult;
		if (tempResult < 0.3) {break;}
	}

	// Take the minimum of the returned values, this is the best match
	float minResult = matchResult[0];
	object=10;
	for (int i=1; i<12; i++)
	{

		if (matchResult[i]<minResult)
		{
			minResult = matchResult[i];
			object = i + 10;
		}
	}

	// If the value for the best match is too high, we decide that we should return UNKNOWN
	if (minResult > 0.45)
	{
		object = 100;
	}


	switch(object)
	{
		case 10: printf("Object is an apple\n"); break;
		case 11: printf("Object is a mug\n"); break;
		case 12: printf("Object is a teddy\n"); break;
		case 13: printf("Object is some glasses\n"); break;
		case 14: printf("Object is a dryer\n"); break;
		case 15: printf("Object is a laptop\n"); break;
		case 16: printf("Object is a glass\n"); break;
		case 17: printf("Object is a hammer\n"); break;
		case 18: printf("Object is a banana\n"); break;
		case 19: printf("Object is a book\n"); break;
		case 20: printf("Object is some scissors\n"); break;
		case 21: printf("Object is a camera\n"); break;
		case 100: printf("I don't know\n"); break;
	}
}

/* Function to receive the image */
void imageCallbackCam0(const sensor_msgs::ImageConstPtr& image_msg)
{
	Mat imgMat;
	cv_bridge::CvImagePtr cv_ptr;
	try
	{
		cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8");
		imgMat = cv_ptr->image;
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
	}
	printf("Tag received \n");

	
	namedWindow( "tag", 1 );
	imshow( "tag", imgMat);
	waitKey(0);
	

	// Do color analysis
	color = -1;
	colorAnalysis(imgMat);
	
	// Make the picture binary
	Mat binary;
	cvtColor(imgMat,binary,CV_RGB2GRAY);
	threshold(binary, binary, 80, 255, THRESH_BINARY);
	
	// Resize the image (to have the same size than the templates)
	Mat imgToProcess;
	resize(binary,imgToProcess,Size(60,60),0,0,INTER_LINEAR);

	//Do template matching
	templateMatching(imgToProcess);

	//Publish the information about the tag
	TagRecognition tag;
	tag.color=color;
	tag.object=object;
	recognitionCam0_pub.publish(tag);
	
	
}

/* Function to receive the image */
void imageCallbackCam1(const sensor_msgs::ImageConstPtr& image_msg)
{
	Mat imgMat;
	cv_bridge::CvImagePtr cv_ptr;
	try
	{
		cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8");
		imgMat = cv_ptr->image;
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
	}
	printf("Tag received \n");

	// Do color analysis
	color = -1;
	colorAnalysis(imgMat);
	
	// Make the picture binary
	Mat binary;
	cvtColor(imgMat,binary,CV_RGB2GRAY);
	threshold(binary, binary, 80, 255, THRESH_BINARY);
	
	// Resize the image (to have the same size than the templates)
	Mat imgToProcess;
	resize(binary,imgToProcess,Size(60,60),0,0,INTER_LINEAR);

	//Do template matching
	templateMatching(imgToProcess);

	//Publish the information about the tag
	TagRecognition tag;
	tag.color=color;
	tag.object=object;
	recognitionCam1_pub.publish(tag);
	
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "ImageProcessing");
	printf("Image processing node \n");
	ros::NodeHandle n;

	recognitionCam0_pub = n.advertise<TagRecognition>("recognition_Cam0", 1);
	recognitionCam1_pub = n.advertise<TagRecognition>("recognition_Cam1", 1);

	image_transport::ImageTransport it(n);
	image_transport::Subscriber sub_imgCam0 = it.subscribe("camera/imageCam0", 1, imageCallbackCam0);
	image_transport::Subscriber sub_imgCam1 = it.subscribe("camera/imageCam1", 1, imageCallbackCam1);

	ros::spin();
}
